/**
 * Version 1.422
 * Uses mecanum drive with rotation axis
 * Arm driver is working and controlled by the relays and the buttons
 * Minibot deployment and retraction is controlled by relays and buttons
 * Deleted old commented code and added comments for meaning
 * Added alpha autonomous code and line tracker code
 * Changed deploy buttons to trigger and 11
 * Crisis mode off
 * Last worked on April 8, 2011 at 9:07 AM
 */
//blank
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.Gyro;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.GenericHID.*;
import edu.wpi.first.wpilibj.Relay;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.DigitalInput;

public class RobotTemplate extends SimpleRobot {

    RBDrive drive = new RBDrive(1, 3, 2, 4); //frontLeft(1), rearLeft(3), frontRight(2), rearRight(4)
    Joystick leftStick = new Joystick(1);
    Joystick rightStick = new Joystick(2);
    Relay armRelay = new Relay(1);
    Relay miniDeploy = new Relay(2);
    DigitalInput topLimit = new DigitalInput(1);
    DigitalInput bottomLimit = new DigitalInput(2);
    DigitalInput deployLimit = new DigitalInput(3);

    //these are assuming we have 3 IR readers for reading reflective tape
    DigitalInput leftReader = new DigitalInput(4);
    DigitalInput midReader = new DigitalInput(5);
    DigitalInput rightReader = new DigitalInput(6);
    //This timer is for dead reconing the distance during autonomous
    Timer time = new Timer();

    //These are failed experements with the axis camera and the gyro.
    //Enable at your own risk

        //Gyro gyro = new Gyro(1);
        //boolean rotating = false; //returns true if the robot is purposefully rotating
        //private AxisCamera camera = AxisCamera.getInstance();

    public void autonomous() { //Still in alpha, run at own risk
        /**
        while (isAutonomous() && isEnabled()) {
            time.start();
            while(time.get() < 5000){ //Gives us 5 seconds
            if(midReader.get() == true){
                drive.drive(1.0, 0.0);
            }

            if(leftReader.get() == true){
                while(midReader.get() != true && leftReader.get() == true){
                drive.drive(0.0, .01 );
                }
            }
            if(rightReader.get() == true){
                while(midReader.get() != true && rightReader.get() == true){
                drive.drive(0.0, -.01 );
                }
            break;
                }
            }

            }
         * **/
        }
    

    public void operatorControl() { //CRISIS-MODE: OFf

        while (isOperatorControl() && isEnabled()) // loop during enabled teleop mode
        {
            //Arm Code
            while (topLimit.get() == true && (leftStick.getRawButton(5) == true || leftStick.getRawButton(6) == true)) { //buttons 5 + 6 corrospond to upward arm rotation
                armRelay.setDirection(Relay.Direction.kForward);
                armRelay.set(Relay.Value.kOn);
                System.out.println("Arm up");
                drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), leftStick.getZ(), 0.0);
                //drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), rightStick.getX(), 0.0);
            }

            while (bottomLimit.get() == true && (leftStick.getRawButton(3) == true || leftStick.getRawButton(4) == true)) { //buttons 3 + 4 corrospond to downward arm rotation
                armRelay.setDirection(Relay.Direction.kReverse);
                armRelay.set(Relay.Value.kOn);
                System.out.println("Arm down");
                drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), leftStick.getZ(), 0.0);
                //drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), rightStick.getX(), 0.0);
            }
            //End Arm Code

          //Minibot Deployment

            while (deployLimit.get() == true && leftStick.getRawButton(1) == true){
                miniDeploy.setDirection(Relay.Direction.kReverse);
                miniDeploy.set(Relay.Value.kOn);
                System.out.println("DEPLOY");
                drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), leftStick.getZ(), 0.0);
                //drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), rightStick.getX(), 0.0);
            }

            while(leftStick.getRawButton(11) == true){
                miniDeploy.setDirection(Relay.Direction.kForward);
                miniDeploy.set(Relay.Value.kOn);
                System.out.print("Reverse");
                drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), leftStick.getZ(), 0.0);
                //drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), rightStick.getX(), 0.0);

            }
            //End Minibot Deployment


            //Defaults the motors to off
            armRelay.set(Relay.Value.kOff);
            miniDeploy.set(Relay.Value.kOff);

            //Used for testing deploy limits
            System.out.println("Deploy Limit Get:" + deployLimit.get());
            Timer.delay(.005); //

            //Drive Code
            drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), leftStick.getZ(), 0.0);
            //drive.mecanumDrive_Cartesian(leftStick.getX(), leftStick.getY(), rightStick.getX(), 0.0);
            Timer.delay(0.005);

        }
    }
}



